/*  A class to handle a camera's geo-oriented calibration. */

// aabbggrr format
const FRUSTUM_COLOR = 'ffb5e4ff';  // pale yellow
//const FRUSTUM_COLOR = 'ff8000ff'; // hot pink
const FRUSTUM_LINE_WIDTH = 4;

function Calibration(M, camCenter, pitch, heading, origin, width, height){
	this.M = M.values;
	this.camCenter = camCenter;
	this.pitch = pitch;
	this.heading = heading;
	this.origin = origin;
	this.width = width;
	this.height = height;
	
	// make the frusta, if you need it.
	if (document.getElementById('edgeLength')){
		this.makeFrustum();
	}

	if (document.getElementById('frustumBox')){
		this.toggleFrustum();
	}
	
	var me = this;
	var timer = null;
	google.earth.addEventListener(ge.getView(), 'viewchangeend', function(){
		if ( timer ) {
		  clearTimeout( timer );
		}
		timer = setTimeout( function() { me.updateFrustum() }, 100 );
      }
    );
}

function normalize(v, center, scale){
	var dx = v.x - center.x;
	var dy = v.y - center.y;
	var dz = v.z - center.z;
	var mult = scale / Math.sqrt(dx * dx + dy * dy + dz * dz)
	return {x: v.x * mult,
	y: v.y * mult,
	z: v.z * mult,
	lat: 0, lon: 0, alt:0}
}

Calibration.prototype.invertM = function(){
    var m3x3 = new Matrix(3,3, [[this.M[0][0], this.M[0][1], this.M[0][2]],
                                [this.M[1][0], this.M[1][1], this.M[1][2]],
                                [this.M[2][0], this.M[2][1], this.M[2][2]]]);
    this.invM = m3x3.invert();
}

// finds the 3D point that projects onto the point
// u, v that is length meters away from the camera
// center.
Calibration.prototype.uv2xyz = function(u,v, length){
	const d = -10000;
	const toDegrees = 180 / Math.PI;
	const toRadians = 1/toDegrees;
	if (this.invM == undefined){
	    this.invertM();
	}
	/*
	var mat = new Matrix(4,3, [[this.M[0][0], this.M[0][1], this.M[0][2], (u * d - this.M[0][3])], 
							   [this.M[1][0], this.M[1][1], this.M[1][2], (v * d - this.M[1][3])],
							   [this.M[2][0], this.M[2][1], this.M[2][2], ( d    - this.M[2][3])]]
							   );
	var vec = mat.rowEchelon().values;
	var pos = {x: vec[0][3], y: vec[1][3], z: vec[2][3], lat:0, lon:0, alt:0};
	*/
	//alert(pos.x + ',' + pos.y + ',' + pos.z);
	
	var vec = new Matrix(1, 3, [[(u * d - this.M[0][3])], 
							   [(v * d - this.M[1][3])],
							   [( d    - this.M[2][3])]]);
    var newPos = this.invM.multiply(vec);
    newPos = newPos.values;
    var pos = {x: newPos[0][0], y: newPos[1][0], z: newPos[2][0]}
    //alert(newPos.x + ',' + newPos.y + ',' + newPos.z);
	
	
	// make sure the 3D point is in front of the camera.
	var camPitch = this.pitch * toRadians;
	var camHeading = this.heading * toRadians;
	var camTheta = PI - camPitch;
	var camPhi = -camHeading + PI / 2;
	var directionX = Math.sin(camTheta) * Math.cos(camPhi);
	var directionY = Math.sin(camTheta) * Math.sin(camPhi);
	var directionZ = Math.cos(camTheta);
	if (pos.x * directionX + pos.y * directionY + pos.z * directionZ < 0){
		pos.x *= -1;
		pos.y *= -1;
		pos.z *= -1;
	}
	
	
	// normalize the length of the vector from the eye to the xyz point.
	var dx = pos.x - this.camCenter.x;
	var dy = pos.y - this.camCenter.y;
	var dz = pos.z - this.camCenter.z;
	var mult = length / Math.sqrt(dx * dx + dy * dy + dz * dz);
	pos.x = dx * mult;
	pos.y = dy * mult;
	pos.z = dz * mult;
	
	// find approximate degrees lat / meter, degrees lon / meter.
	var latPerMeter = (0.000001)/distance(this.origin.lat,         this.origin.lon,         this.origin.alt, 
										  this.origin.lat+0.000001, this.origin.lon,         this.origin.alt);
	var lonPerMeter = (0.000001)/distance(this.origin.lat,         this.origin.lon,         this.origin.alt,
										  this.origin.lat,         this.origin.lon+0.000001, this.origin.alt);
	
	
	pos.lon = pos.x * lonPerMeter + this.camCenter.lon;
	pos.lat = pos.y * latPerMeter + this.camCenter.lat;
	pos.alt = pos.z + this.camCenter.alt;
	
	return pos
}

// returns the (u,v) point that the geographic point projects onto.
Calibration.prototype.geoToUV = function(geo){
    var dLon = geo.lon - this.origin.lon;
    var dLat = geo.lat - this.origin.lat;
    
    // find approximate degrees lat / meter, degrees lon / meter.
	var meterPerLat = distance(this.origin.lat,         this.origin.lon,         this.origin.alt, 
							   this.origin.lat+0.000001, this.origin.lon,         this.origin.alt) / (0.000001);
	var meterPerLon = distance(this.origin.lat,         this.origin.lon,         this.origin.alt,
							   this.origin.lat,         this.origin.lon+0.000001, this.origin.alt) / (0.000001);
										  
	var x = dLon * meterPerLon;
	var y = dLat * meterPerLat;
	var z = geo.alt;
	var M = this.M;
	var uw = M[0][0] * x + M[0][1] * y + M[0][2] * z + M[0][3];
	var vw = M[1][0] * x + M[1][1] * y + M[1][2] * z + M[1][3];
	var w  = M[2][0] * x + M[2][1] * y + M[2][2] * z + M[2][3];
	
	return {u: uw/w, v:vw/w};
}

Calibration.prototype.makeFrustum = function(){
	var length = document.getElementById('edgeLength').value;
	this.TL = this.uv2xyz(0,0,length);
	this.TR = this.uv2xyz(this.width,0,length);
	this.BR = this.uv2xyz(this.width,this.height,length);
	this.BL = this.uv2xyz(0,this.height,length);
}

Calibration.prototype.drawFrustum = function(){
	// draw to GE.
	var lineTL = ge.createLineString('');
	lineTL.getCoordinates().pushLatLngAlt(this.TL.lat, this.TL.lon, this.TL.alt);
	lineTL.getCoordinates().pushLatLngAlt(this.camCenter.lat, this.camCenter.lon, this.camCenter.alt);
	lineTL.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
	
	var lineTR = ge.createLineString('');
	lineTR.getCoordinates().pushLatLngAlt(this.TR.lat, this.TR.lon, this.TR.alt);
	lineTR.getCoordinates().pushLatLngAlt(this.camCenter.lat, this.camCenter.lon, this.camCenter.alt);
	lineTR.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
	
	var lineBR = ge.createLineString('');
	lineBR.getCoordinates().pushLatLngAlt(this.BR.lat, this.BR.lon, this.BR.alt);
	lineBR.getCoordinates().pushLatLngAlt(this.camCenter.lat, this.camCenter.lon, this.camCenter.alt);
	lineBR.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
	
	var lineBL = ge.createLineString('');
	lineBL.getCoordinates().pushLatLngAlt(this.BL.lat, this.BL.lon, this.BL.alt);
	lineBL.getCoordinates().pushLatLngAlt(this.camCenter.lat, this.camCenter.lon, this.camCenter.alt);
	lineBL.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
	
	var lineImage = ge.createLineString('');
	lineImage.getCoordinates().pushLatLngAlt(this.BL.lat, this.BL.lon, this.BL.alt);
	lineImage.getCoordinates().pushLatLngAlt(this.BR.lat, this.BR.lon, this.BR.alt);
	lineImage.getCoordinates().pushLatLngAlt(this.TR.lat, this.TR.lon, this.TR.alt);
	lineImage.getCoordinates().pushLatLngAlt(this.TL.lat, this.TL.lon, this.TL.alt);
	lineImage.getCoordinates().pushLatLngAlt(this.BL.lat, this.BL.lon, this.BL.alt);
	lineImage.setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
	
	var currentCamera = ge.getView().copyAsCamera(ge.ALTITUDE_ABSOLUTE);
	var dist = distance(this.camCenter.lat, this.camCenter.lon, this.camCenter.alt, currentCamera.getLatitude(), currentCamera.getLongitude(), currentCamera.getAltitude());
	
	
	var multiGeometry = ge.createMultiGeometry('');
	if (dist > 0.1){ // if you're close enough to the actual camera location, then don't draw the supporting
	                 // lines.  You get weird graphics bugs when a line goes into the camera.
	    multiGeometry.getGeometries().appendChild(lineTR);
	    multiGeometry.getGeometries().appendChild(lineBR);
	    multiGeometry.getGeometries().appendChild(lineTL);
	    multiGeometry.getGeometries().appendChild(lineBL);
    }
	multiGeometry.getGeometries().appendChild(lineImage);
	
	//this.undrawFrustum();
	var newPlacemark = ge.createPlacemark('');
	newPlacemark.setGeometry(multiGeometry);
	
	// Create a style and set width and color of line
	newPlacemark.setStyleSelector(ge.createStyle(''));
	var lineStyle = newPlacemark.getStyleSelector().getLineStyle();
	lineStyle.setWidth(FRUSTUM_LINE_WIDTH);
	lineStyle.getColor().set(FRUSTUM_COLOR);
		
	if (this.frustumPlacemark){
	    ge.getFeatures().removeChild(this.frustumPlacemark);
	} 
	ge.getFeatures().appendChild(newPlacemark);
	
	this.frustumPlacemark = newPlacemark;
}

Calibration.prototype.undrawFrustum = function(){
	if (this.frustumPlacemark){
	    
		ge.getFeatures().removeChild(this.frustumPlacemark);
	}
}

Calibration.prototype.updateFrustum = function(){
	if (this.frustumOn){
	    
		//this.undrawFrustum();
		this.makeFrustum();
		this.drawFrustum();
	}
}

Calibration.prototype.toggleFrustum = function(){
	this.frustumOn = document.getElementById('frustumBox').checked;
	if (this.frustumOn){
		this.drawFrustum();
	} else {
		this.undrawFrustum();
	}
}

Calibration.prototype.fitModelToFrustum = function(pm, heading, tilt, roll){
	const TORADIANS = 3.14159 / 180;
	pm.getGeometry().getLocation().setLatLngAlt(this.BL.lat, this.BL.lon, this.BL.alt);
	pm.getGeometry().getOrientation().set(heading, tilt, roll);
	
	var dx = (this.TL.x - this.TR.x);
	var dy = (this.TL.y - this.TR.y);
	var dz = (this.TL.z - this.TR.z);
	var scaleX = Math.sqrt(dx*dx + dy*dy + dz*dz);
	
	dx = (this.TL.x - this.BL.x);
	dy = (this.TL.y - this.BL.y);
	dz = (this.TL.z - this.BL.z);
	var scaleY = Math.sqrt(dx*dx + dy*dy + dz*dz);
	pm.getGeometry().getScale().set(scaleX, -scaleY,1);
}

